#include "MS5611.h"
#include <math.h>

#define MS5611_ADDR             0xee //    0x77

#define CMD_RESET               0x1E // ADC reset command
#define CMD_ADC_READ            0x00 // ADC read command
#define CMD_ADC_CONV            0x40 // ADC conversion command
#define CMD_ADC_D1              0x00 // ADC D1 conversion
#define CMD_ADC_D2              0x10 // ADC D2 conversion
#define CMD_ADC_256             0x00 // ADC OSR=256
#define CMD_ADC_512             0x02 // ADC OSR=512
#define CMD_ADC_1024            0x04 // ADC OSR=1024
#define CMD_ADC_2048            0x06 // ADC OSR=2048
#define CMD_ADC_4096            0x08 // ADC OSR=4096
#define CMD_PROM_RD             0xA0 // Prom read command
#define PROM_NB                 8
#define MS5611_OSR							0x08	//CMD_ADC_4096

vs32  BaroAlt,BaroOffset;
uint32_t ms5611_ut;  // static result of temperature measurement
uint32_t ms5611_up;  // static result of pressure measurement
uint16_t ms5611_prom[PROM_NB];  // on-chip ROM

static void ms5611_reset(void)
{
	u8 data = 1;
    ANO_TC_I2C1_Write_Buf(MS5611_ADDR, CMD_RESET, 1, &data);
    //delay_nus(2800);
}
static void ms5611_read_prom(void)
{
    uint8_t rxbuf[2] = { 0, 0 };
		for (u8 i = 0; i < PROM_NB; i++)
		{
			ANO_TC_I2C1_Read_Buf(MS5611_ADDR, CMD_PROM_RD + i * 2, 2, rxbuf); // send PROM READ command
			ms5611_prom[i] = rxbuf[0] << 8 | rxbuf[1];
		}
}
static uint32_t ms5611_read_adc(void)
{
    uint8_t rxbuf[3];
    ANO_TC_I2C1_Read_Buf(MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
    return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
}
static void ms5611_start_ut(void)
{
	u8 data = 1;
    ANO_TC_I2C1_Write_Buf(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + MS5611_OSR, 1, &data); // D2 (temperature) conversion start!
}
static void ms5611_get_ut(void)
{
    ms5611_ut = ms5611_read_adc();
}
static void ms5611_start_up(void)
{
	u8 data = 1;
    ANO_TC_I2C1_Write_Buf(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + MS5611_OSR, 1, &data); // D1 (pressure) conversion start!
}
static void ms5611_get_up(void)
{
    ms5611_up = ms5611_read_adc();
}
static int32_t ms5611_calculate(void)
{
    int32_t temperature, off2 = 0, sens2 = 0, delt;
    int32_t pressure;

    int32_t dT = ms5611_ut - ((uint32_t)ms5611_prom[5] << 8);
    int64_t off = ((uint32_t)ms5611_prom[2] << 16) + (((int64_t)dT * ms5611_prom[4]) >> 7);
    int64_t sens = ((uint32_t)ms5611_prom[1] << 15) + (((int64_t)dT * ms5611_prom[3]) >> 8);
    temperature = 2000 + (((int64_t)dT * ms5611_prom[6]) >> 23);

    if (temperature < 2000) { // temperature lower than 20degC 
        delt = temperature - 2000;
        delt = delt * delt;
        off2 = (5 * delt) >> 1;
        sens2 = (5 * delt) >> 2;
        if (temperature < -1500) { // temperature lower than -15degC
            delt = temperature + 1500;
            delt = delt * delt;
            off2  += 7 * delt;
            sens2 += (11 * delt) >> 1;
        }
    }
    off  -= off2; 
    sens -= sens2;
    pressure = (((ms5611_up * sens ) >> 21) - off) >> 15;
		pressure = (int)((1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f); // centimeter
    return pressure;
}
#define BARO_CAL_CNT 50
vs32 sum_temp = 0;
u8   sum_cnt = 0;
void MS5611_Cal(void)
{
	static u8 state=0;
	
	switch(state)
	{
		case 0:	ms5611_reset();
						state++;
						break;
		case 1: ms5611_read_prom();
						state++;
						break;
		case 2:	ms5611_start_ut();
						state++;
						break;
		case 3:	ms5611_get_ut();
						ms5611_start_up();
						state++;
						break;
		case 4:	ms5611_get_up();
						BaroAlt = ms5611_calculate()-BaroOffset;
						if(sum_cnt)
						{
							sum_cnt--;
							sum_temp += BaroAlt;
							if(sum_cnt==0)
								BaroOffset = sum_temp / BARO_CAL_CNT;
						}
						ms5611_start_ut();
						state=3;
						break;
		default:ms5611_reset();
						state=1;
						break;
	}
}
void MS5611_CalOffset(void)
{
	BaroOffset = 0;
	sum_temp = 0;
	sum_cnt = BARO_CAL_CNT;
}
